#include "ros/ros.h"
#include <geometry_msgs/Twist.h>         // 指线速度角速度的消息类型
#include <tf/transform_listener.h>       // 用于坐标转换
#include <nav_msgs/Path.h>                  // 显示历史轨迹和路径
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <nav_msgs/Odometry.h>           // 用于读取里程计信息
#include <sensor_msgs/LaserScan.h>       // lidar
#include <robust_formation/leaderMpc.h>                 // MPC解优化
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>

// inlcude iostream and string libraries
#include <iostream>
#include <map>      // 包含了key-value键值对映射的排序分配容器
#include <math.h>
#include <string>
#include <vector>

using namespace std;
using namespace Eigen;

constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

/********************/
/* CLASS DEFINITION */
/********************/
class MPCNode
{
    public:
        MPCNode();  // 构造函数
        ~MPCNode(); // 析构函数
        int get_thread_numbers();   // 获取线程数
    
    private:
        ros::NodeHandle _nh;
        ros::Subscriber _sub_odom, _sub_distance, _sub_cmd, _sub_cleantime;
        ros::Publisher _pub_totalcost, _pub_twist, _pub_mpctraj, _pub_mpcdif;
        ros::Timer _timer1;

        nav_msgs::Odometry _odom;
        geometry_msgs::Twist _twist_msg, _sub_twist_cmd;
        nav_msgs::Path _mpc_traj;

        string _map_frame, _odom_frame, _car_frame;
        string _leader_frame;

        BCI_MPC _mpc;
        map<string, double> _mpc_params;
        double _mpc_steps, _w_linvel, _w_linvel_d, _w_angvel, _w_angvel_d, _max_angvel, _max_speed, _bound_value;

        double _dt, _w, _speed;
        int _controller_freq, _thread_numbers;
        bool _debug_info, _delay_mode;
        float _closest_distance, _closest_degree;                               // 雷达测量障碍物的最近距离和角度
        double _target1_x, _target1_y, _target2_x, _target2_y;                  // set target A(5, 5) B(5, -5)
        double distanceToA, distanceToB;                                        // 任意时刻Leader与目标的距离

        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);               // 订阅里程计消息回调函数
        void controlLoopCB(const ros::TimerEvent&);                             // 控制回调函数
        void get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);   // 订阅雷达消息回调函数
        void read_cmd(const geometry_msgs::Twist::ConstPtr& cmd);               // 读取用户输入的命令
        void clean_time_CB(const std_msgs::Int32::ConstPtr& msg);               // 将当前时间归零

        // Intention tracking control
        void bci_ctrl();   

        unsigned int idx; 
        double _time_begin;       // 记录开始时间
        double _cur_time;         // 当前时间
        std_msgs::Int32 cwm_flag; // 记录当前状态（到达A、B或者超时）

        geometry_msgs::Twist _mpc_dif; // 记录mpc变化情况
}; // end of class

// 构造函数
MPCNode::MPCNode()
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Parameters for control loop
    pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node
    pn.param("debug_info", _debug_info, true);
    pn.param("delay_mode", _delay_mode, false);
    pn.param("controller_freq", _controller_freq, 4);
    _dt = double(1.0/_controller_freq); // time step duration dt in s 

    //Parameter for MPC solver
    pn.param("mpc_steps", _mpc_steps, 4.0);
    pn.param("mpc_w_angvel", _w_angvel, 5.0);
    pn.param("mpc_w_angvel_d", _w_angvel_d, 1.0);
    pn.param("mpc_w_linvel", _w_linvel, 100.0);
    pn.param("mpc_w_linvel_d", _w_linvel_d, 1.0);    
    pn.param("max_speed", _max_speed, 0.04);// unit: m/s
    pn.param("max_angvel", _max_angvel, 0.2); // Maximal angvel radian 
    pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables

    //Parameter for topics & Frame name
    pn.param<std::string>("map_frame", _map_frame, "tb3_0/odom" ); //*****for mpc, "odom"
    pn.param<std::string>("odom_frame", _odom_frame, "tb3_0/odom");
    pn.param<std::string>("car_frame", _car_frame, "tb3_0/base_footprint" );
    pn.param<std::string>("leader_frame", _leader_frame, "tb3_0" );

    // set targets
    pn.param("target1_x", _target1_x, 5.0);
    pn.param("target1_y", _target1_y, 5.0);
    pn.param("target2_x", _target2_x, 5.0);
    pn.param("target2_y", _target2_y, -5.0);

    //Display the parameters
    cout << "\n===== Parameters =====" << endl;
    cout << "frequency: "   << _dt << endl;
    cout << "mpc_steps: "   << _mpc_steps << endl;
    cout << "mpc_w_linvel: "   << _w_linvel << endl;
    cout << "mpc_w_linvel_d: "  << _w_linvel_d << endl;    
    cout << "mpc_w_angvel: "   << _w_angvel << endl;
    cout << "mpc_w_angvel_d: "  << _w_angvel_d << endl;
    cout << "mpc_max_angvel: "  << _max_angvel << endl;
    cout << "mpc_max_speed: "  << _max_speed << endl;

    //Publishers and Subscribers
    _sub_odom   = _nh.subscribe(_leader_frame + "/odom", 1, &MPCNode::odomCB, this);                   // 订阅里程计消息
    _sub_distance   = _nh.subscribe(_leader_frame + "/processed_scan", 1, &MPCNode::get_laser_callback, this);   // 订阅雷达消息
    // _sub_cmd = _nh.subscribe("/cmd_udp", 1, &MPCNode::read_cmd, this); // change 20210113
    _sub_cmd = _nh.subscribe("/cmd_user", 1, &MPCNode::read_cmd, this); // change 20210113
    _sub_cleantime = _nh.subscribe("/publishCleanTime", 1, &MPCNode::clean_time_CB, this);

    _pub_twist = _nh.advertise<geometry_msgs::Twist>(_leader_frame + "/cmd_vel", 1);    // publish speed msg
    _pub_totalcost  = _nh.advertise<std_msgs::Float32>("/total_cost", 1);               // total_cost
    _pub_mpctraj = _nh.advertise<nav_msgs::Path>("/mpc_trajectory", 1);   // MPC trajectory output  /  Prediction time domain
    _pub_mpcdif = _nh.advertise<geometry_msgs::Twist>("/mpc_dif", 1);  // 发布MPC的变化情况
    
    //Timer
    _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &MPCNode::controlLoopCB, this); // 4Hz //*****mpc

    //Init parameters for MPC object
    _mpc_params["DT"] = _dt;
    _mpc_params["STEPS"]    = _mpc_steps;
    _mpc_params["W_LINVEL"]  = _w_linvel;
    _mpc_params["W_DLINVEL"] = _w_linvel_d;    
    _mpc_params["W_ANGVEL"]  = _w_angvel;
    _mpc_params["W_DANGVEL"] = _w_angvel_d;
    _mpc_params["MAX_ANGVEL"]   = _max_angvel;
    _mpc_params["MAX_SPEED"]   = _max_speed;
    _mpc_params["BOUND"]    = _bound_value;
    _mpc.LoadParams(_mpc_params);

    idx = 0;         // 求解次数

    _w = 0.0;        // 角速度
    _speed = 0.0;    // 速度

    _closest_degree = 0;        // 初始化最近障碍物角度和距离
    _closest_distance = 3.5;

    _twist_msg = geometry_msgs::Twist();
    _mpc_traj = nav_msgs::Path();

    _time_begin = ros::Time::now().toSec();
    while (_time_begin == 0){
        _time_begin = ros::Time::now().toSec();
    }
    cout << "time_begin: " << _time_begin << endl;
}

MPCNode::~MPCNode()
{
    // 析构函数  
}

// Public: return _thread_numbers
int MPCNode::get_thread_numbers()
{
    return _thread_numbers;
}

// Timer: Control Loop (closed loop nonlinear MPC)
void MPCNode::controlLoopCB(const ros::TimerEvent&)
{       
    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    const double autonomy = cmd_user.linear.z;
    double curent_time = ros::Time::now().toSec() - _time_begin;
    cout << "duration: " << curent_time << endl; 
    bci_ctrl();
}

void MPCNode::bci_ctrl() {
    // input variables
    nav_msgs::Odometry odom = _odom;
    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    const double cmd_v = cmd_user.linear.x;       // Linear velocity
    const double cmd_w = cmd_user.angular.z;      // angular velocity

    // user's command
    VectorXd vec_cmd(2);
    vec_cmd << cmd_v , cmd_w;
    _mpc_dif.linear.x = cmd_v;
    _mpc_dif.linear.y = cmd_w;

    // Update system states: X=[x, y, theta]
    const double px = odom.pose.pose.position.x; //pose: odom frame
    const double py = odom.pose.pose.position.y;
    tf::Pose pose;
    tf::poseMsgToTF(odom.pose.pose, pose);
    const double theta = tf::getYaw(pose.getRotation()); 
    const double v = odom.twist.twist.linear.x; //twist: body fixed frame 
    const double w = odom.twist.twist.angular.z; //angular_velocity

    // distance and degree 距离障碍物的最近距离和角度
    const double min_distance = _closest_distance;  
    const double min_degree = _closest_degree;    

    // Leader当前时刻与目标的距离
    distanceToA = sqrt((px - _target1_x) * (px - _target1_x) + (py - _target1_y) * (py - _target1_y)); 
    distanceToB = sqrt((px - _target2_x) * (px - _target2_x) + (py - _target2_y) * (py - _target2_y));

    // 当前时间
    _cur_time = ros::Time::now().toSec() - _time_begin;

    // 判断条件： 离目标较远， 时间不到5分钟
    if (_cur_time < 600.0) {
        // state
        VectorXd state(5);
        state << 0, 0, 0, min_distance, min_degree;

        // Solve MPC Problem
        vector<double> mpc_results = _mpc.Solve(state, vec_cmd);

        // MPC result (all described in car frame), output = (w)  
        _speed = mpc_results[0];            // Linear velocity
        _w = mpc_results[1];                // radian/sec, angular velocity

        cwm_flag.data = 0;
    } else {
        _speed = 0.0;                       // Linear velocity
        _w = 0.0;                           // radian/sec, angular velocity
        cout << "Time out !!!!!!!" << endl;   
        cwm_flag.data = 3; 
    }

    // publish general cmd_vel
    // _twist_msg.linear.x = _speed;
    // _twist_msg.angular.z = _w;
    _twist_msg.linear.x  = _speed; 
    _twist_msg.angular.z = _w;
    _pub_twist.publish(_twist_msg);

    _mpc_dif.angular.x = _speed;
    _mpc_dif.angular.y = _w;
    _pub_mpcdif.publish(_mpc_dif);

    std_msgs::Float32 mpc_total_cost;
    mpc_total_cost.data = static_cast<float>(_mpc._mpc_totalcost);
    _pub_totalcost.publish(mpc_total_cost);

    // Display the MPC predicted trajectory
    _mpc_traj = nav_msgs::Path();
    _mpc_traj.header.frame_id = _car_frame; // points in car coordinate        
    _mpc_traj.header.stamp = ros::Time::now();
    for(int i=0; i<_mpc.mpc_x.size(); i++)
    {
        geometry_msgs::PoseStamped tempPose;
        tempPose.header = _mpc_traj.header;
        tempPose.pose.position.x = _mpc.mpc_x[i];
        tempPose.pose.position.y = _mpc.mpc_y[i];
        tempPose.pose.orientation.w = 1.0;
        _mpc_traj.poses.push_back(tempPose); 
    }     
    // publish the mpc trajectory
    _pub_mpctraj.publish(_mpc_traj);

    
    // idx++;
    // cout << "debug:01 " << endl;
    // cout << "idx: "<< idx << endl;
    cout << "Leader X: " << px << endl;
    cout << "Leader Y: " << py << endl;
    cout << "Leader V: " << v << endl;
    cout << "Leader W: " << w << endl;
    cout << endl;
    // cout << "distanceToA: " << distanceToA << endl;
    // cout << "distanceToB: " << distanceToB << endl;
}

// CallBack: Update odometry
void MPCNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    _odom = *odomMsg;
}

// 读取用户输入的命令 
void MPCNode::read_cmd(const geometry_msgs::Twist::ConstPtr& cmd)
{
    _sub_twist_cmd = *cmd;
}
// 将当前时间清零
void MPCNode::clean_time_CB(const std_msgs::Int32::ConstPtr& msg)
{
    int key = msg->data;
    // cout << "key: " << key << endl;
    if (key == 0) {
        _time_begin = ros::Time::now().toSec();
        while (_time_begin == 0){
            _time_begin = ros::Time::now().toSec();
        }
        // cout << "time_begin： " << _time_begin << endl;
    }

}

// 获取雷达最近距离和角度
void MPCNode::get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // 获取距离数组
    std::vector<float> ranges=msg->ranges;
    float angle_min1 = msg -> angle_min;
    float angle_max1 = msg -> angle_max;

    // 获取距离数组的长度
    int len_ = ranges.size();

    // 距离数组标准化
    for(int i=0 ; i < len_ ; i++)
    {
        if(isinf(ranges[i]))
        {
            ranges[i] = 3.5;
        }
        else if(isnan(ranges[i]))
        {
            ranges[i] = 0.0;
        }
    }

    // 求最短距离
    auto closest_range = std::min_element(ranges.begin(),ranges.end());
    float closest_range_ = *(closest_range);

    // 求最短距离所在位置(角度rad)
    auto closest_range_seq = closest_range - ranges.begin() ;
    float closest_degree_rad = angle_min1 + closest_range_seq*(angle_max1-angle_min1)/len_  ;

    // changed
    if((0.785 < closest_degree_rad)&&(closest_degree_rad < 5.495))
    {
        closest_range_ = 3.5;
    }
    //float closest_degree = rad2deg(closest_degree_rad);

    _closest_distance = closest_range_;
    _closest_degree = closest_degree_rad;

    // 显示
    /*
    std::cout << "最短距离：" << closest_range_ << "  ";
    std::cout << "角度：" << closest_degree << "  ";
    std::cout << "弧度：" << closest_degree_rad << std::endl;
    */
}

/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "leaderMpcNode");

    MPCNode mpc_node;

    ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}
